//
// Created by lqk on 2022/1/30.
//

#ifndef QUADCEREBELLUM_QUADRUPEDKINEMATICS_H
#define QUADCEREBELLUM_QUADRUPEDKINEMATICS_H
#include "QuadrupedSpecifiction.h"
#include "Eigen/Core"
#include "LegStructs.h"

class LegKinematics{
public:
    LegKinematics(const LegSpecification& leg): m_leg(leg){}
    Eigen::Matrix3f CalculateJacobian(const Eigen::Vector3f & q,int sideSign);
    Eigen::Vector3f CalculateForwardKinematics(const Eigen::Vector3f & q,int sideSign);
    Eigen::Vector3f CalculateInverseKinematics(const Eigen::Vector3f & desiredPos, int sideSign);
private:
    LegSpecification m_leg;
};

class QuadrupedKinematics{
public:
    QuadrupedKinematics(const QuadrupedSpecification& quadSpec):m_quadSpec(quadSpec),m_legFRKinematics(quadSpec.m_legSpec),m_legFLKinematics(quadSpec.m_legSpec),m_legBRKinematics(quadSpec.m_legSpec), m_legBLKinematics(quadSpec.m_legSpec){}
    void CalculateJacobians(LegState* const);
    void CalculateFootsPosition(LegState* const);
    void CalculateInverseKinematics();
    Eigen::Vector3f GetHipLocation(int leg);

private:
    QuadrupedSpecification m_quadSpec;
    LegKinematics m_legFRKinematics;
    LegKinematics m_legFLKinematics;
    LegKinematics m_legBRKinematics;
    LegKinematics m_legBLKinematics;




};


#endif //QUADCEREBELLUM_QUADRUPEDKINEMATICS_H
